#include "pid.hpp"

pid_pos::pid_pos(double Kp,
                 double Ki,
                 double Kd,
                 double set_value,
                 double integral_max_limit,
                 double integral_min_limit,
                 double output_max_limit,
                 double output_min_limit)
{
    _Kd = Kd;
    _Kp = Kp;
    _Ki = Ki;
    _set_value = set_value;
    _integral_max_limit = integral_max_limit;
    _integral_min_limit = integral_min_limit;
    _output_max_limit = output_max_limit;
    _output_min_limit = output_min_limit;
    debug_flag = false;
    _previous_error = 0;
    _integral = 0;

    if (output_max_limit < output_min_limit || integral_max_limit < integral_min_limit)
    {
        LOG(ERROR) << "pid_pos controller simple init pra error";
        goto end;
    }

    init_flag = YJ_OK;
    // LOG(INFO) << "pid_pos controller simple init ok";
    return;
end:
    init_flag = YJ_ERROR;
    LOG(ERROR) << "pid_pos controller simple init fail";
    return;
}

void pid_pos::reset_value()
{
    std::lock_guard<std::mutex> lock(_data_lock);
    _previous_error = 0;
    _integral = 0;
}

double pid_pos::update(double value)
{
    double error, derivative;

    std::lock_guard<std::mutex> lock(_data_lock);
    error = _set_value - value;
    _now_error_at.store(error, std::memory_order_release);
    _now_in_value_at.store(value, std::memory_order_release);

    derivative = error - _previous_error;
    _integral += _Ki * error;
    // 累计超过限幅处理
    if (_integral > _integral_max_limit)
        _integral = _integral_max_limit;
    else if (_integral < _integral_min_limit)
        _integral = _integral_min_limit;

    double output = _Kp * error + _Ki * _integral + _Kd * derivative;
    _previous_error = error;

    if (debug_flag)
    {
        LOG(INFO) << "KP: " << _Kp << " KI: " << _Ki << " KD: " << _Kd;
        LOG(INFO) << "set_value: " << _set_value << " input value: " << value;
        LOG(INFO) << "pid_pos output: " << output << " error: " << error << " integral: " << _integral << " derivative error: " << derivative;
    }

    return output;
}

double pid_pos::update_with_I_death_zone(double value, double I_death_value)
{
    double error, derivative;

    std::lock_guard<std::mutex> lock(_data_lock);
    error = _set_value - value;
    _now_error_at.store(error, std::memory_order_release);
    _now_in_value_at.store(value, std::memory_order_release);

    derivative = error - _previous_error;

    double output;
    if (abs(I_death_value) < abs(error))
    {
        _integral += _Ki * error;
        // 累计超过限幅处理
        if (_integral > _integral_max_limit)
            _integral = _integral_max_limit;
        else if (_integral < _integral_min_limit)
            _integral = _integral_min_limit;

        output = _Kp * error + _Ki * _integral + _Kd * derivative;
    }
    else
    {
        _integral = 0;
        output = _Kp * error + _Kd * derivative;
    }

    _previous_error = error;

    if (debug_flag)
        LOG(INFO) << "pid_pos output: " << output << " error: " << error << " integral: " << _integral << " derivative error: " << derivative;

    return output;
}

double pid_pos::output_limit(double update_value)
{
    std::lock_guard<std::mutex> lock(_data_lock);
    if (update_value < _output_min_limit)
        return _output_min_limit;
    else if (update_value > _output_max_limit)
        return _output_max_limit;
    else
        return update_value;
}

double pid_pos::output_limit(double update_value, double max_limit, double min_limit)
{

    if (max_limit < min_limit)
    {
        LOG(ERROR) << "output_limit max limit < min_limit, no limit applied";
        return update_value;
    }
    if (debug_flag)
        LOG(INFO) << "update_value: " << update_value << " max_limit: " << max_limit << " min_limit: " << min_limit;

    if (update_value < min_limit)
        return min_limit;
    else if (update_value > max_limit)
        return max_limit;
    else
        return update_value;
}

void pid_pos::get_pid_value(double &Kp, double &Ki, double &Kd)
{
    std::lock_guard<std::mutex> lock(_data_lock);
    Kp = _Kp;
    Ki = _Ki;
    Kd = _Kd;
}
void pid_pos::set_pid_value(double Kp, double Ki, double Kd)
{
    std::lock_guard<std::mutex> lock(_data_lock);
    _Kp = Kp;
    _Ki = Ki;
    _Kd = Kd;
    _previous_error = 0;
    _integral = 0;
}

void pid_pos::set_des_value(double set_value)
{
    std::lock_guard<std::mutex> lock(_data_lock);
    _set_value_at.store(set_value, std::memory_order_release);
    _set_value = set_value;
}

void pid_pos::set_output_limit(double max_limit, double min_limit)
{
    std::lock_guard<std::mutex> lock(_data_lock);
    _output_max_limit = max_limit;
    _output_min_limit = min_limit;
}

yj_return pid_pos::read_init_status()
{
    return init_flag;
}

void pid_pos::open_debug()
{
    debug_flag = true;
}

double pid_pos::get_des_value()
{
    return _set_value_at.load(std::memory_order_acquire);
}
double pid_pos::get_error_value()
{
    return _now_error_at.load(std::memory_order_acquire);
}
double pid_pos::get_update_in_value()
{
    return _now_in_value_at.load(std::memory_order_acquire);
}

// 双环PID
pid_double_ring::pid_double_ring(pid_pos *in_pid, pid_pos *out_pid) : _in_pid(in_pid),
                                                                      _out_pid(out_pid),
                                                                      _out_pid_max_limit(0),
                                                                      _out_pid_min_limit(0),
                                                                      debug_flag(false)
{
    if (!in_pid || !out_pid)
    {
        LOG(ERROR) << "pid_double_ring init failed, in_pid or out_pid is nullptr";
        init_flag = YJ_ERROR;
        return;
    }

    if (in_pid->read_init_status() != YJ_OK || out_pid->read_init_status() != YJ_OK)
    {
        LOG(ERROR) << "pid_double_ring init failed, in_pid or out_pid init failed";
        init_flag = YJ_ERROR;
        return;
    }

    init_flag = YJ_OK;
}

void pid_double_ring::set_des_value(double set_value)
{
    _out_pid->set_des_value(set_value);
}

void pid_double_ring::set_out_pid_limit_value(double max_limit, double min_limit)
{
    std::lock_guard<std::mutex> lock(_data_lock);
    _out_pid_max_limit = max_limit;
    _out_pid_min_limit = min_limit;
}

double pid_double_ring::update(double out_value, double in_value)
{
    double update_temp;
    {
        std::lock_guard<std::mutex> lock(_data_lock);
        update_temp = _out_pid->update(out_value);
        update_temp = _out_pid->output_limit(update_temp, _out_pid_max_limit, _out_pid_min_limit);
    }

    // LOG(INFO) << "update to in: " << update_temp << " in_value: " << in_value;
    // LOG(INFO) << "_out_pid set value: "<< _out_pid->get_des_value();
    if (debug_flag)
        LOG(INFO) << "update_temp: " << update_temp << " out_value: " << out_value;

    _in_pid->set_des_value(update_temp);
    update_temp = _in_pid->update(in_value);
    update_temp = _in_pid->output_limit(update_temp);
    // LOG(INFO) << "output update_temp: " << update_temp;

    return update_temp;
}

double pid_double_ring::update_with_death_zone(double out_value, double in_value, double in_pid_death_zone)
{
    double update_temp;
    {
        std::lock_guard<std::mutex> lock(_data_lock);
        update_temp = _out_pid->update(out_value);
        update_temp = _out_pid->output_limit(update_temp, _out_pid_max_limit, _out_pid_min_limit);
    }

    // LOG(INFO) << "update to in: " << update_temp << " in_value: " << in_value;
    // LOG(INFO) << "_out_pid set value: "<< _out_pid->get_des_value();
    if (debug_flag)
        LOG(INFO) << "update_temp: " << update_temp << " out_value: " << out_value;

    _in_pid->set_des_value(update_temp);
    update_temp = _in_pid->update_with_I_death_zone(in_value, in_pid_death_zone);
    update_temp = _in_pid->output_limit(update_temp);
    // LOG(INFO) << "output update_temp: " << update_temp;

    return update_temp;
}

yj_return pid_double_ring::read_init_status()
{
    return init_flag;
}

void pid_double_ring::open_debug()
{
    debug_flag = true;
}

double pid_double_ring::get_des_value()
{
    return _out_pid->get_des_value();
}
double pid_double_ring::get_error_value()
{
    return _out_pid->get_error_value();
}
double pid_double_ring::get_update_in_value()
{
    return _out_pid->get_update_in_value();
}